from numpy import *
from scipy.integrate import odeint
import numpy as np
import math
import matplotlib.pyplot as plt
import pylab as p
from mpl_toolkits.mplot3d import axes3d
import eqn_of_motion as eqn
import time

init_state=array([0,0,0,0,0,0,0,0,0,0,0,0])

t=arange(0,10.0,.01)

#dstate=eqn.eqn_deriv(init_state,t)
sol=odeint(eqn.eqn_deriv, init_state, t)

#Plot Results
plt.figure(1)
plt.subplot(311)
plt.plot(t,sol[:,2])
plt.xlabel('Time (s)')
plt.ylabel('Altitude (m)')
plt.subplot(312)
plt.plot(t,sol[:,0])
plt.xlabel('Time (s)')
plt.ylabel('xposition (m)')
plt.subplot(313)
plt.plot(t,sol[:,1])
plt.xlabel('Time (s)')
plt.ylabel('yposition (m)')
plt.show()

#plt.figure(2)
#plt.subplot(311)
#plt.plot(t,sol[:,6])
#plt.xlabel('Time (s)')
#plt.ylabel('Roll (rad)')
#plt.subplot(312)
#plt.plot(t,sol[:,7])
#plt.xlabel('Time (s)')
#plt.ylabel('Pitch (rad)')
#plt.subplot(313)
#plt.plot(t,sol[:,8])
#plt.xlabel('Time (s)')
#plt.ylabel('Yaw (rad)')
#plt.show()

#plt.ion()
#fig = plt.figure()
#ax = fig.add_subplot(111, projection='3d')

##at each timepoint
#i=0
#frame=None
#frame2=None

#for dt in t:
#	oldframe = frame
#	oldframe2 = frame2
#
#	phi = sol[i,6]
#	theta = sol[i,7]
#	psi = sol[i,8]
#	x = sol[i,0]
#	y = sol[i,1]
#	z = sol[i,2]
#	print dt

##Define trigonometrics to simplify rotation matrices
#	cphi = math.cos(phi)
#	sphi = math.sin(phi)
#	cth = math.cos(theta)
#	sth = math.sin(theta)
#	cpsi = math.cos(psi)
#	spsi = math.sin(psi)
	
#Rotation Matrix
#	R = array( [(cth*cpsi, sphi*sth*cpsi-cphi*spsi, cphi*sth*cpsi+sphi*spsi),
#				(cth*spsi, sphi*sth*spsi+cphi*cpsi, cphi*sth*spsi-sphi*cpsi),
#				(sth,      -sphi*cth,               -cphi*cth)] )
#
#	r1=array([1, 0, 0])
#	r2=array([-1, 0, 0])
#	r3=array([0, 1, 0])
#	r4=array([0, -1, 0])
#
#	#rotate vectors per rotation matrix:
#	r1new = dot(R,r1)
#	r2new = dot(R,r2)
#	r3new = dot(R,r3)
#	r4new = dot(R,r4)
#
#	frame = ax.plot([r1new[0]+x,r2new[0]]+x,[r1new[1]+y,r2new[1]+y],[r1new[2]+z,r2new[2]+z])
#	frame2 = ax.plot([r3new[0]+x,r4new[0]]+x,[r3new[1]+y,r4new[1]+y],[r3new[2]+z,r4new[2]+z])
#	ax.set_xlim3d(-10,10)
#	ax.set_ylim3d(-10,10)
#	ax.set_zlim3d(-10,10)
#	
#	#print oldframe
#	
#	plt.draw()
#	ax.lines=[]
#	i+=1
#plt.draw()
plt.ioff()
#print t
#print sol[:,2]